#include "Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h" // Device header
#include "stm32f4xx_conf.h"

#define Servo1 1 //PD12
#define Servo2 2 //PD13
#define Servo3 3 //PD14
#define Servo4 4 //PD15
#define Servo5 5 //PA2
#define Servo6 6 //PA3



/**
  * @brief  舵机初始化
  * @param  无
  * @retval 无
  */
void Servo_Init(void) {
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;
    
    // 1. 使能时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
    // 2. 配置GPIO
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14| GPIO_Pin_15;//PD12,PD13,PD14,PD15
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOD, &GPIO_InitStruct);
	
		GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;//PA2,PA3
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(GPIOA, &GPIO_InitStruct);
    
    // 引脚复用映射
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);  // PD12->TIM4_CH1
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);  // PD13->TIM4_CH2
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_TIM4);  // PD14->TIM4_CH3
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_TIM4);  // PD15->TIM4_CH4
		GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM5);  // PA2->TIM5_CH3
		GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM5);  // PA3->TIM5_CH4
    // 3. 配置定时器时基
    TIM_TimeBaseStruct.TIM_Prescaler = 1680;    //
    TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseStruct.TIM_Period = 1000 - 1; //
    TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStruct);
    TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStruct);
    // 4. 配置PWM模式
    TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
		TIM_OCInitStruct.TIM_Pulse=75;//初始时在正中间
    
    // PD12
    TIM_OC1Init(TIM4, &TIM_OCInitStruct);
    TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
    
    // PD13
    TIM_OC2Init(TIM4, &TIM_OCInitStruct);
    TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
    
    // PD14
    TIM_OC3Init(TIM4, &TIM_OCInitStruct);
    TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
		// PD15
    TIM_OC4Init(TIM4, &TIM_OCInitStruct);
    TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
		// PA2
    TIM_OC3Init(TIM5, &TIM_OCInitStruct);
    TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);
		// PA3
    TIM_OC4Init(TIM5, &TIM_OCInitStruct);
    TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable);
    
    // 5. 启动定时器
    TIM_Cmd(TIM4, ENABLE);
    TIM_ARRPreloadConfig(TIM4, ENABLE);  // 使能ARR预装载
		TIM_Cmd(TIM5, ENABLE);
    TIM_ARRPreloadConfig(TIM5, ENABLE);  // 使能ARR预装载
}


/**
  * @brief  舵机角度设置
  * @param  舵机编号，角度值
  * @retval 无
  */
void Servo_Angle(uint8_t Servo_Num,float angle){
	uint16_t pulse_width;
			if(angle < -90.0f) angle = -90.0f;
			if(angle > 90.0f) angle= 90.0f;
			pulse_width= (angle+90)/1.8+25;

	switch (Servo_Num){
		case Servo1:
			TIM4->CCR1 = pulse_width;
			TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
		break;
		case Servo2:
			TIM4->CCR2 = pulse_width;
			TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
		break;
		case Servo3:
			TIM4->CCR3 = pulse_width;
			TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
		break;
		case Servo4:
			TIM4->CCR4 = pulse_width;
			TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
		break;
		case Servo5:
			TIM5->CCR3 = pulse_width;
			TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);
		break;
		case Servo6:
			TIM5->CCR4 = pulse_width;
			TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable);
		break;
	}
	
}


